#include "tsdf.h"
namespace Registration {

__global__ void update_tsdf_kernel(uint16_t *depth_image, int image_width,
                                   int image_height, short2 *tsdf_volume,
                                   int3 volume_size, float voxel_scale,
                                   CameraParams camera_params,
                                   const float truncation_distance,
                                   float *rotation, float *tanslation) {

  const int x = blockIdx.x * blockDim.x + threadIdx.x;
  const int y = blockIdx.y * blockDim.y + threadIdx.y;
  if (x >= volume_size.x || y >= volume_size.y)
    return;
  for (int z = 0; z <= volume_size.z; z++) {
    float3 voxel_center =
        make_float3((static_cast<float>(x) + 0.5) * voxel_scale,
                    (static_cast<float>(y) + 0.5) * voxel_scale,
                    (static_cast<float>(z) + 0.5) * voxel_scale);
    float3 position_in_camera_coord =
        transform(voxel_center, rotation, tanslation);

    if (position_in_camera_coord.z <= 0)
      continue;
    float2 uv;
    uv.x = (camera_params.focal_x * position_in_camera_coord.x) /
               position_in_camera_coord.z +
           camera_params.principal_x;
    uv.y = (camera_params.focal_y * position_in_camera_coord.y) /
               position_in_camera_coord.z +
           camera_params.principal_y;
    if (uv.x < 0 || uv.x >= image_width || uv.y < 0 || uv.y >= image_height)
      continue;
    int idx = static_cast<int>(uv.x) * image_width + static_cast<int>(uv.y);
    uint16_t depth_value = depth_image[idx];
    if (depth_value <= 0)
      continue;

    float3 xylamada;
    xylamada.x = (uv.x - camera_params.principal_x) / camera_params.focal_x;
    xylamada.y = (uv.y - camera_params.principal_y) / camera_params.focal_y;
    xylamada.z = 1.0f;

    float lamada = norm(xylamada);
    float distance = norm(position_in_camera_coord);
    const float sdf = -1.0f * ((1.0f / lamada) * distance - depth_value);

    if (sdf >= -truncation_distance) {
      const float new_tsdf = fmin(1.0f, sdf / truncation_distance);
      int index = z * volume_size.x * volume_size.y + y * volume_size.x + x;
      short2 voxel_tuple = tsdf_volume[index];
      const float current_tsdf =
          static_cast<float>(voxel_tuple.x) * DIVSHORTMAX;
      const int current_wieght = voxel_tuple.y;
      const int add_weight = 1;
      const float update_tsdf =
          (current_wieght * current_tsdf + add_weight * new_tsdf) /
          (current_wieght + add_weight);
      const int new_weight =
          fmin(float(current_wieght + add_weight), float(MAX_WEIGHT));
      const int new_value =
          fmax(float(-SHORTMAX),
               fmin(float(SHORTMAX), float(update_tsdf * SHORTMAX)));
      tsdf_volume[index] = make_short2(new_value, new_weight);
    }
  }
}

__device__ float3 transform(float3 target_point, const float *rotate_matrix,
                            const float *translation_matrix) {
  float3 rotate_point;
  rotate_point.x = rotate_matrix[0] * target_point.x +
                   rotate_matrix[1] * target_point.y +
                   rotate_matrix[2] * target_point.z;
  rotate_point.y = rotate_matrix[3] * target_point.x +
                   rotate_matrix[4] * target_point.y +
                   rotate_matrix[5] * target_point.z;
  rotate_point.z = rotate_matrix[6] * target_point.x +
                   rotate_matrix[7] * target_point.y +
                   rotate_matrix[8] * target_point.z;
  float3 result_point = make_float3(rotate_point.x + translation_matrix[0],
                                    rotate_point.y + translation_matrix[1],
                                    rotate_point.z + translation_matrix[2]);
  return result_point;
}

__device__ float norm(float3 input_vector) {
  float length =
      sqrt(input_vector.x * input_vector.x + input_vector.y * input_vector.y +
           input_vector.z * input_vector.z);
  return length;
}

void surfce_reconstruction(uint16_t *depth_image, int image_width,
                           int image_height, VolumeData &volume,
                           const CameraParams &cam_params,
                           const float truncation_distance, float *rotation,
                           float *translation) {
  // int voxel_
  /*
    const dim3 threads(32, 32);
    const dim3 blocks((image_width + threads.x - 1) / threads.x,
                      (image_height + threads.y - 1) / threads.y);
    update_tsdf_kernel<<<blocks, threads>>>(
        depth_image, image_width, image_height, volume.tsdf_data,
        volume.VolumeSize, volume.voxel_scale, cam_params, truncation_distance,
        rotation, translation);
    cudaThreadSynchronize();*/
}


__global__ void merge_point_cloud(
	PointXyz *target_point_cloud,
	int target_point_num,
	VolumeData *voxel_data,
	int3 voxel_size,
	float3 vaxel_scale,
	int *match_index_array)
{
	int idx = blockIdx.x * blockDim.x + threadIdx.x;
	while (idx<target_point_num)
	{
		int match_index = match_index_array[idx];
		if (match_index==-1)
		{
			idx += blockDim.x * gridDim.x;
			continue;
		}
		PointXyz source_point = voxel_data[match_index].voxel_point;
		int weight= voxel_data[match_index].weight;
		PointXyz target_point = target_point_cloud[idx];
		voxel_data[match_index].voxel_point.point.x = (source_point.point.x*weight + target_point.point.x) / (weight + 1);
		voxel_data[match_index].voxel_point.point.y = (source_point.point.y*weight + target_point.point.y) / (weight + 1);
		voxel_data[match_index].voxel_point.point.z = (source_point.point.z*weight + target_point.point.z) / (weight + 1);
		voxel_data[match_index].weight += 1;
		idx += blockDim.x * gridDim.x;
	}

}





} // namespace Registration